import torch
device = torch.device("cuda:0" if torch.cuda.is_available() else "cpu")
"""
常量定义
"""
action_scale = 0.25
control_type = "P"
stiffness = 40.0  # [N*m/rad]
damping = 1.0  # [N*m*s/rad]

torque_limit = 12.

default_pos = torch.tensor([ 0.0, 0.9, -1.8,
                        0.0, 0.9, -1.8, 
                        0.0, 1.12, -1.8,
                        0.0, 1.12, -1.8, ]).to(device)
"""
FL -> FR -> RL -> RR
"""


def compute_torques(actions, dof_pos, dof_vel):
    """Compute torques from actions.
        Actions can be interpreted as position or velocity targets given to a PD controller, or directly as scaled torques.
        [NOTE]: torques must have the same dimension as the number of DOFs, even if some DOFs are not actuated.

    Args:
        actions (torch.Tensor): Actions

    Returns:
        [torch.Tensor]: Torques sent to the simulation
    """
    # pd controller
    actions_scaled = actions * action_scale
    torques = (
        stiffness * (actions_scaled + default_pos - dof_pos)
        - damping * dof_vel
    )

    return torch.clip(torques, -torque_limit, torque_limit)

